

#include"mll_SVM.h"

#include"sl_.h"  // 用了链表做队列，缓存支持向量
#include"stl_.h" // 用了矩阵缓存核函数
#include"gl_.h"  // 用函数绘图可视化svm


#include<stdlib.h>
#include<math.h>



/// <summary>
///  核函数
/// </summary>
typedef float (*KernelFunction)(vec xi, vec xj);


//整形的双向队列。
typedef struct _dlist_node_int_ {
	/// <summary>
	/// 指向链表节点的指针
	/// </summary>
	DListNode       node;          //。
	/// <summary>
	/// 数据
	/// </summary>
	int data;       //    
} DListNode_int;




/// <summary>
/// svm 的数据结构。
/// </summary>
typedef struct _SVM___
{
	KernelFunction kf;   // 核函数
	vec* x;    // x是数据矢量。
	float* y;  // y == 1  或 -1，是数据的标签。
	float* alpha; // 学习的参数。	
	float b;

	int N;    // 上面数组的长度。
	// 分类模型：   h(x) = sign( sum( alpha_i*y_i*KernelFunction(x_i,x) ) + b  )
	float C;   // 错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。值越小，分类间隔会变大，但可能会有一些错误分类的点。
}SVM;

// 不用追求太极致的效率……

/// <summary>
/// 创建核函数矩阵，避免重复计算
/// </summary>
/// <param name="x">数据矢量数组，数组长度是K.cols == K.rows</param>
/// <param name="kf">核函数的函数指针</param>
/// <param name="K">核函数矩阵</param>
void create_K(vec* x, KernelFunction kf, mat K)
{
	int data_length = K.cols;
	for (int i = 0; i < data_length; i++) {
		for (int j = 0; j < data_length; j++) {
			K.data[i][j] = kf(x[i], x[j]);
		}
	}
}





/// <summary>
/// 计算优化的目标值，SMO算法使目标值越来越小。
/// 刷新alpha数组变量后，需要重新计算误差数组e。
/// </summary>
/// <param name="y">数据集标签== 1  或 -1，数组长度是K.cols == K.rows </param>
/// <param name="e">数据集元素在当前参数下的误差值数组</param>
/// <param name="alpha">学习的参数（决策函数的系数）</param>
/// <param name="d0C">支持向量的索引</param>
/// <param name="K">svm的参数，核函数矩阵</param>
/// <param name="b">svm的参数，</param>
/// <param name="C">svm的参数，错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。</param>
/// <returns> 目标函数值： sum_ij( alpha[i]*alpha[j]*y[i]*y[j]*Kij ) - 2 * sum_i( alpha[i] ) </returns>
float target_with_err_(float* y, float* e, float* alpha, DList d0C, mat K, float b, float C)
{
	int data_length = K.cols;
	float t = 0;
	float g;
	float ajyj;
	float aiyi;

	// 遍历链表
	DListNode* cursori;
	DListNode* cursorj;
	int i, j;
	for (cursori = d0C.head; cursori != NULL; cursori = cursori->next)
	{
		i = (container_of(cursori, DListNode_int, node))->data;
		e[i] = 0;
		aiyi = alpha[i] * y[i];
		for (cursorj = d0C.head; cursorj != NULL; cursorj = cursorj->next)
		{
			j = (container_of(cursorj, DListNode_int, node))->data;
			ajyj = alpha[j] * y[j];
			g = ajyj * K.data[i][j];
			e[i] += g;
			t += g * aiyi;
		}
		e[i] += (b - y[i]);
		t -= alpha[i] * 2;
	}

	return t;
}


int discriminant_functions(SVM* svm, vec x)
{
	int data_length = svm->N;
	float g = svm->b;
	float ay;
	float aiyi = 0;
	float C_epsilon = svm->C - Epsilon;
	for (int j = 0; j < data_length; j++) {
		//	if (svm->alpha[j] < C_epsilon)
		{
			ay = svm->alpha[j] * svm->y[j];
			g += ay * svm->kf(x, svm->x[j]); //  K.data[i][j];
		}
	}
	return g;
}

/// <summary>
/// 二次规划算法，跟新alpha参数。这里是更新选中的两个点的参数，这两个点的索引是 a1,a2,
/// 更新后的值是a1new，a2new。
/// </summary>
/// <param name="alpha">模型的alpha数组</param>
/// <param name="y">数据集标签== 1  或 -1，数组长度是K.cols == K.rows </param>
/// <param name="e">数据集元素在当前参数下的误差值数组</param>
/// <param name="K">svm的参数，核函数矩阵</param>
/// <param name="C">svm的参数，错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。</param>
/// <param name="a1">第一个点的索引</param>
/// <param name="a2">第二个点的索引</param>
/// <param name="a1new">第一个点的新alpha值</param>
/// <param name="a2new">第二个点的新alpha值</param>
/// <param name="d_a2"> 这个参数记录 a2 点alpha的变化量(alpha[a2] - *a2new)，如果变化太小，就要换个点来更新alpha </param>
void renew_a(float* alpha, float* y, float* e, mat K, float C,
	int a1, int a2, float* a1new, float* a2new, float* d_a2)
{
	float L;
	float H;
	float a2new_unc;

	if (ABS(y[a1] - y[a2]) < Epsilon) //     (y[a1] != y[a2] )  //  y = 1 或 -1，可以用整形来存储，减少浮点运算。  (ABS(y[a1] - y[a2]) < Epsilon) 
	{
		L = Max(0, alpha[a1] + alpha[a2] - C);
		H = Min(C, alpha[a1] + alpha[a2]);
	}
	else {
		L = Max(0, alpha[a2] - alpha[a1]);
		H = Min(C, alpha[a2] - alpha[a1] + C);
	}
	float n = (K.data[a1][a1] + K.data[a2][a2] - 2 * K.data[a1][a2]);
	// if (n < 0) printfs(" n----- ");

	a2new_unc = alpha[a2] + y[a2] * (e[a1] - e[a2]) / n;
	*a2new = (a2new_unc > H) ? H : ((a2new_unc < L) ? L : a2new_unc);
	*d_a2 = (alpha[a2] - *a2new);
	*a1new = alpha[a1] + y[a1] * y[a2] * (*d_a2);
}

/// <summary>
/// 二次规划算法，更新b参数。
/// </summary>
/// <param name="alpha">模型的alpha数组</param>
/// <param name="y">数据集标签== 1  或 -1，数组长度是K.cols == K.rows </param>
/// <param name="e">数据集元素在当前参数下的误差值数组</param>
/// <param name="K">svm的参数，核函数矩阵</param>
/// <param name="C">svm的参数，错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。</param>
/// <param name="a1">第一个点的索引</param>
/// <param name="a2">第二个点的索引</param>
/// <param name="a1new">第一个点的新alpha值</param>
/// <param name="a2new">第二个点的新alpha值</param>
/// <param name="b">更新的b的值</param>
void renew_b(float* alpha, float* y, float* e, mat K, float C,
	int a1, int a2, float a1new, float a2new, float* b)
{
	float d_a2 = (alpha[a2] - a2new);
	float d_a1 = (alpha[a1] - a1new);
	bool is_b1 = false;
	bool is_b2 = false;
	float C_Epsilon = C - Epsilon;
	if (Epsilon < a1new && a1new < C_Epsilon)
	{
		is_b1 = true;
	}
	if (Epsilon < a2new && a2new < C_Epsilon)
	{
		is_b2 = true;
	}
	float	b1 = -e[a1] + y[a1] * K.data[a1][a1] * d_a1 + y[a2] * K.data[a2][a1] * d_a2 + *b;
	float	b2 = -e[a2] + y[a1] * K.data[a1][a2] * d_a1 + y[a2] * K.data[a2][a2] * d_a2 + *b;

	if (is_b1)
	{
		*b = b1;
	}
	else if (is_b2)
	{
		*b = b2;
	}
	else
	{
		*b = (b1 + b2) / 2;
	}
}


//内存分配。 data_Dimension:数据集的维度。
void svm_allocate(SVM* svm, int data_length, int data_Dimension)
{
	/*
		KernelFunction kf;   // 核函数
		vec* x;    // x是数据矢量。
		float* y;  // y == 1  或 -1，是数据的标签。
		float* alpha; // 学习的参数。
		float b;

		int N;    // 上面数组的长度。
		// 分类模型：   h(x) = sign( sum( alpha_i*y_i*KernelFunction(x_i,x) ) + b  )
		float C;   // 错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。值越小，分类间隔会变大，但可能会有一些错误分类的点。
	*/
	svm->N = data_length;
	svm->x = (vec*)malloc(svm->N * sizeof(vec));
	for (int i = 0; i < svm->N; i++)
	{
		vec_allocate(&(svm->x[i]), data_Dimension);
	}
	svm->y = (real_t*)malloc(svm->N * sizeof(real_t));
	svm->alpha = (real_t*)malloc(svm->N * sizeof(real_t));
}

//回收内存
void svm_deallocate(SVM* svm)
{
	if (svm->y == NULL) {
		free(svm->y);
		svm->y = NULL;
	}
	if (svm->alpha == NULL) {
		free(svm->alpha);
		svm->alpha = NULL;
	}
	if (svm->x == NULL) {
		for (int i = 0; i < svm->N; i++) {
			vec_deallocate(&svm->x[i]);
		}
		free(svm->x);
		svm->x = NULL;
	}
}
//


/// <summary>
/// 用SMO算法，根据数据集和标签计算svm
/// </summary>
/// <param name="svm">svm的结构体，如果已经有初始值，则将原有的数据和新的数据一起学习</param>
/// <param name="x">数据集</param>
/// <param name="y">标签集合，取值= -1 或 1</param>
/// <param name="data_length">数据集的长度</param>
/// <param name="C">错误分类的代价系数。 这个值越大，错误分类的代价越高，训练结果会越准确，但分类间隔会越小。</param>
/// <param name="kf">核函数的函数指针</param>
/// <returns>训练的迭代次数</returns>
int init_SVM_by_smo_(SVM* svm, vec* x, float* y, int data_length, float C, KernelFunction kf)
{
	vec v_alpha, v_e;
	mat K;
	vec_allocate(&v_alpha, data_length);
	vec_allocate(&v_e, data_length);
	mat_allocate(&K, data_length, data_length);

	vec_init(v_alpha, 0);  // 初始化参数为0
	create_K(x, kf, K);

	/// alpha = 0 的索引
	DList list_0;   //
	/// C >= alpha > 0 的索引
	DList list_0C;   //
	dlist_init(&list_0, NULL);
	dlist_init(&list_0C, NULL);

	DListNode_int* dindex = (DListNode_int*)malloc(sizeof(DListNode_int) * data_length);
	for (int i = 0; i < data_length; i++) {  // 初始化节点。
		dindex[i].data = i;
		dlist_push_back(&list_0, &(dindex[i].node));
		v_e.data[i] = -y[i];
	}

	float precision = 5;   // 精度。迭代的时候，参数的变化量大于precision才会被选择。 这个值不能设置太小，否则会在局部震荡。
	// float precision = Epsilon;

	float b = 0;
	float* alpha = v_alpha.data;
	float* e = v_e.data;
	float err = -1;
	float err0 = -1;

	// 下面开始训练
	int t = 0;
	//	mat_print(K);
	int C_epsilon = C - Epsilon;
	while (1)
	{
		t++;

		// 遍历链表// 寻找最优的 alpha_1
		int a1 = -1, a2 = -1;
		int a1_list = 1, a2_list;  //  0 表示这个索引在list_0的链表，1表示这个索引在list_0C的链表。
		//int e_max_index;
		//int e_min_index;
		DListNode* cursori;
		float a1new;
		float a2new;
		float d_a2;

		float delta_a2 = 0;
		float max_delta_a2 = Epsilon * 10;


		// 如果list_0C.count == 0， 说明刚初始化， 定有 alpha[]=0,b=0 >> gi=0 >> gi*yi=0, 不满足gi*yi >=1 ,此时直接选择索引1，2的变量即可。
		if (list_0C.count == 0) {
			a1_list = 0;
			a2_list = 0;
			a1 = 0;
			for (int i = 0; i < data_length; i++) {
				if (y[i] != y[0]) {
					a2 = i;
					break;
				}
			}
		}
		else
		{
			DListNode* temp_node = NULL;
			float bias = 0;  // 计算最大的偏差。
			for (cursori = list_0C.head; cursori->next != NULL; cursori = cursori->next)
			{
				int i = (container_of(cursori, DListNode_int, node))->data;
				if (alpha[i] < C_epsilon) {   //  alpha[i] < C >>  ei*yi = 0  ,abs(ei) 就是偏差。
					if (ABS(e[i] > bias)) {
						bias = e[i];
						a1 = i;
						temp_node = cursori;
					}
				}
				else    //   alpha[i] == C >>  ei*yi <= 0 ,如果两个同号，就不满足KKT调节
				{
					if (e[i] > 0 && y[i] > 0 && e[i] > bias) {
						bias = e[i];
						a1 = i;
						temp_node = cursori;
					}
					else if (e[i] < 0 && y[i] < 0 && (-e[i]) > bias) {
						bias = -e[i];
						a1 = i;
						temp_node = cursori;
					}
				}
			}
			if (a1 != -1) // 如果在 list_0C 找到a1
			{
				a1_list = 1;
				dlist_remove(&list_0C, temp_node);
				dlist_push_back(&list_0C, temp_node);
			}
			else  // 如果在 list_0C 未找到a1 ，在list_0 继续找。 不满足KKT调节： ei*yi < 0  的点。即异号的点 
			{
				bias = 0;
				for (cursori = list_0.head; cursori->next != NULL; cursori = cursori->next)
				{
					int i = (container_of(cursori, DListNode_int, node))->data;
					if (e[i] > 0 && y[i] < 0 && e[i] > bias) {
						bias = e[i];
						a1 = i;
						temp_node = cursori;
					}
					else if (e[i] < 0 && y[i] > 0 && (-e[i]) > bias) {
						bias = -e[i];
						a1 = i;
						temp_node = cursori;
					}
				}
				if (a1 != -1) {   // 找到了a1
					a1_list = 0;
					dlist_remove(&list_0, temp_node);
					dlist_push_back(&list_0, temp_node);
				}
				else {
					break;   // 还是没找到，说明全部满足KKT调节，可以退出循环。
				}
			}

			//   这里肯定找到了a1，如果没找到就退出去了。        

			a2 = -1;
			// 在 list_0C 找 a2
			cursori = list_0C.head;
			while (cursori != NULL)
			{
				int i = (container_of(cursori, DListNode_int, node))->data;
				if (e[a1] != e[i]) {
					renew_a(alpha, y, e, K, C, a1, i, &a1new, &a2new, &delta_a2);
					//	float da1 = ABS(a1 - a1new);
					if (ABS(delta_a2) > max_delta_a2) {
						max_delta_a2 = ABS(delta_a2);
						a2 = i;
						a2_list = 1;
					}
				}
				cursori = cursori->next;
			}
			// 在 list_0C 中没找到，在 list_0 中寻找。
			if (a2 == -1) {
				cursori = list_0.head;
				while (cursori != NULL)
				{
					int i = (container_of(cursori, DListNode_int, node))->data;
					if (e[a1] != e[i]) {
						renew_a(alpha, y, e, K, C, a1, i, &a1new, &a2new, &delta_a2);
						if (ABS(delta_a2) > max_delta_a2) {
							max_delta_a2 = ABS(delta_a2);
							a2 = i;
							a2_list = 0;
						}
					}
					cursori = cursori->next;
				}
			}

		}


		err = target_with_err_(y, e, alpha, list_0C, K, b, C);
		if (t % 1000 == 99) {
			printfs(" \r\n k= %4d, err = %9f ,delta_a2 = %9f , a1= %5d, a2= %5d,  list_0C count = %5d  ", t, err, max_delta_a2,
				a1, a2,
				list_0C.count
			);
			if (ABS(err - err0) < precision)break;

			err0 = err;
		}


		// 这里肯定找到了a1，如果没找到a2，换一个a1。
		if (a2 != -1) {

			renew_a(alpha, y, e, K, C, a1, a2, &a1new, &a2new, &d_a2);
			renew_b(alpha, y, e, K, C, a1, a2, a1new, a2new, &b);
			// 刷新变量：
			alpha[a1] = a1new;
			alpha[a2] = a2new;
			if (a1new > Epsilon && a1_list == 0) {
				dlist_remove(&list_0, &(dindex[a1]).node);
				dlist_push_back(&list_0C, &(dindex[a1].node));
			}
			if (a1new <= Epsilon && a1_list == 1) {
				dlist_remove(&list_0C, &(dindex[a1]).node);
				dlist_push_back(&list_0, &(dindex[a1].node));
			}

			if (a2new > Epsilon && a2_list == 0) {
				dlist_remove(&list_0, &(dindex[a2].node));
				dlist_push_back(&list_0C, &(dindex[a2].node));
			}
			if (a2new <= Epsilon && a2_list == 1) {
				dlist_remove(&list_0C, &(dindex[a2].node));
				dlist_push_back(&list_0, &(dindex[a2].node));
			}
		}

	}

	// 下面将支持向量赋值给svm，只选择支持向量：C > v_alpha > 0
	int svm_len = 0;
	for (int i = 0; i < data_length; i++) {
		if (alpha[i] > Epsilon) {
			svm_len++;
		}
	}
	printfs(" \r\n data_length = %4d, svm_len = %d ", data_length, svm_len);

	svm_allocate(svm, svm_len, x[0].size);
	svm->kf = kf;
	int j = 0;
	for (int i = 0; i < data_length; i++) {
		if (alpha[i] > Epsilon) {
			svm->alpha[j] = alpha[i];
			vec_copyfrom(svm->x[j], x[i]);
			svm->y[j] = y[i];
			printfs(" \r\n  svm->x[j] d0 = %9f, svm->x[j] d1 = %9f,  svm->y[j]  = %9f  alpha = %9f ",
				svm->x[j].data[0], svm->x[j].data[1], svm->y[j], svm->alpha[j]);
			j++;
		}
	}
	svm->b = b;
	svm->N = svm_len;
	svm->C = C;

	printfs("  \r\n b = %9f, n = %5d,  C = %9f  ", svm->b, svm->N, svm->C);

	//	svm_deallocate(svm);

	vec_deallocate(&v_alpha);
	vec_deallocate(&v_e);
	mat_deallocate(&K);
	free(dindex);
	return t;
}



// 这个得结合函数绘制才比较好验证。

float kf(vec xi, vec xj)
{
	float a = 0;
	float sigma = 1;
	for (int i = 0; i < xi.size; i++) {
		a += (xi.data[i] - xj.data[i]) * (xi.data[i] - xj.data[i]);
	}
	a = -2.1f * (a / (sigma * sigma));
	return expf(a);
}



void create_data_set()
{
#define data_len 2000

	vec x[data_len];
	float y[data_len];

	for (int i = 0; i < data_len; i++) {
		vec_allocate(&x[i], 2);
		x[i].data[0] = random11();
		x[i].data[1] = random11();
		float a = x[i].data[0];
		float b = 0.5 * c_sin(a * 7); // +0.05 * c_sin(a);
		// float b = a *a* 2; // +0.05 * c_sin(a);

		if (x[i].data[1] > b) {
			y[i] = 1;
			//	x[i].data[1] += 1;
		}
		else {
			y[i] = -1;
			//	x[i].data[1] -= 1;
		}
	}

	float fx[data_len * 2];
	float fy[data_len * 2];
	COLOR_RGB color[data_len * 2];
	for (int i = 0; i < data_len; i++) {
		fx[i] = x[i].data[0];
		fy[i] = x[i].data[1];
		if (y[i] > 0)color[i] = RGB_green;
		else color[i] = RGB_red;
	}
	TwoFunctionPicture tp;
	TwoFunctionPicture_init(&tp, 0, 0, 700, 700);
	char title[] = "Curve Chart";          //标题
	char xtitle[] = "xtitle time";          //x轴标题
	char y1title[] = "y1title cm";          //y轴标题
	char y2title[] = "y2title km";          //y轴标题
	TwoFunctionPicture_draw_title(&tp, title, xtitle, y1title, y2title);

	/*
 */
	TwoFunctionPicture_draw_func(&tp,
		fx, fy, data_len,
		NULL, NULL, 0,
		0, 0, 0, color, NULL);
//	_getch();	// 按任意键继续
	GD_wait_key_input();
	SVM svm;
	init_SVM_by_smo_(&svm, x, y, data_len, 200, kf);


	for (int i = 0; i < data_len; i++) {
		fx[i] = x[i].data[0];
		fy[i] = x[i].data[1];

		// 计算判别函数赋值
		if (discriminant_functions(&svm, x[i]) > 0)
		{
			color[i] = RGB_green;
		}
		else
		{
			color[i] = RGB_red;
		}
		//if (y[i] > 0)color[i] = RGB_green;
		//else color[i] = RGB_red;
	}

	for (int i = 0; i < svm.N; i++) {
		fx[data_len + i] = svm.x[i].data[0];
		fy[data_len + i] = svm.x[i].data[1];
		color[data_len + i] = RGB_blue;

		//	printfs(" \r\n \r\n x = %f, y = %f,  ly = %f  ", svm.x[i].data[0], svm.x[i].data[1], svm.y[i]);
	}
	svm_deallocate(&svm);


	TwoFunctionPicture_draw_func(&tp,
		fx, fy, data_len + svm.N,
		NULL, NULL, 0,
		0, 0, 0, color, NULL);
//	_getch();	// 按任意键继续
	GD_wait_key_input();


	for (int i = 0; i < data_len; i++) {
		vec_deallocate(&x[i]);
	}

}



